#include "actuator/vcs.h"

std::shared_ptr<boost::circular_buffer<st_par>> cls_lib::read_buffer_  = nullptr;
std::shared_ptr<boost::circular_buffer<st_par>> cls_lib::write_buffer_ = nullptr;

cls_lib::cls_lib(std::string &libpath)
{
    const char *f_name = libpath.c_str();
    handle = dlopen(libpath.c_str(), RTLD_LAZY);

    if(!handle)
    {
        fprintf(stderr, "%s\n", dlerror());
        _exit(-1);
    }

    subscribe_int = (type_subscribe_int)dl_adrress_get(handle, "subscribe_int");
    subscribe_float = (type_subscribe_float)dl_adrress_get(handle, "subscribe_float");
    subscribe_bool = (type_subscribe_bool)dl_adrress_get(handle, "subscribe_bool");
    log_send = (type_log_send)dl_adrress_get(handle, "log_send");
    error_send = (type_error_send)dl_adrress_get(handle, "error_send");

    run_server_trig = (type_run_server_trig)dl_adrress_get(handle, "run_server_trig");
}

cls_lib::~cls_lib()
{
    dlclose(handle);
}

void cls_lib::Init(std::shared_ptr<boost::circular_buffer<st_par>> &read_buf, std::shared_ptr<boost::circular_buffer<st_par>> &write_buf)
{
    subscribe_int_init("radar_type", par.radar_type);
    subscribe_int_init("radar_upsidedown", par.radar_upsidedown);
	subscribe_int_init("radar_qty", par.radar_qty);
	subscribe_int_init("radar_pos_x_1", par.radar_pos_x_1);
	subscribe_int_init("radar_pos_y_1", par.radar_pos_y_1);
	subscribe_float_init("radar_pos_z_1", par.radar_pos_z_1);
	subscribe_int_init("radar_pos_x_2", par.radar_pos_x_2);
	subscribe_int_init("radar_pos_y_2", par.radar_pos_y_2);
	subscribe_float_init("radar_pos_z_2", par.radar_pos_z_2);
	subscribe_int_init("radar_pos_x_3", par.radar_pos_x_3);
	subscribe_int_init("radar_pos_y_3", par.radar_pos_y_3);
	subscribe_float_init("radar_pos_z_3", par.radar_pos_z_3);
	subscribe_int_init("radar_pos_x_4", par.radar_pos_x_4);
	subscribe_int_init("radar_pos_y_4", par.radar_pos_y_4);
	subscribe_float_init("radar_pos_z_4", par.radar_pos_z_4);
	subscribe_int_init("imu_pos_x", par.imu_pos_x);
	subscribe_int_init("imu_pos_y", par.imu_pos_y);
	subscribe_int_init("imu_pos_z", par.imu_pos_z);
	subscribe_int_init("imu_pos_dir", par.imu_pos_dir);
	subscribe_float_init("odo_velocity_x", par.odo_velocity_x);
	subscribe_float_init("odo_velocity_y", par.odo_velocity_y);
	subscribe_float_init("odo_velocity_z", par.odo_velocity_z);
	subscribe_int_init("odo_ts_low", par.odo_ts_low);
	subscribe_int_init("odo_ts_high", par.odo_ts_high);
	subscribe_int_init("imu_acc_x", par.imu_acc_x);
	subscribe_int_init("imu_acc_y", par.imu_acc_y);
	subscribe_int_init("imu_acc_z", par.imu_acc_z);
	subscribe_int_init("imu_ang_x", par.imu_ang_x);
	subscribe_int_init("imu_ang_y", par.imu_ang_y);
	subscribe_int_init("imu_ang_z", par.imu_ang_z);
	subscribe_int_init("imu_ts_low", par.imu_ts_low);
	subscribe_int_init("imu_ts_high", par.imu_ts_high);
	subscribe_int_init("safty_forward_0", par.safty_forward_0);
	subscribe_int_init("safty_right_0", par.safty_right_0);
	subscribe_int_init("safty_backward_0", par.safty_backword_0);
	subscribe_int_init("safty_left_0", par.safty_left_0);
	subscribe_int_init("safty_forward_1", par.safty_forward_1);
	subscribe_int_init("safty_right_1", par.safty_right_1);
	subscribe_int_init("safty_backward_1", par.safty_backword_1);
	subscribe_int_init("safty_left_1", par.safty_left_1);
	subscribe_int_init("slam_pos_x", par.slam_pos_x);
	subscribe_int_init("slam_pos_y", par.slam_pos_y);
	subscribe_float_init("slam_pos_z", par.slam_pos_z);
	subscribe_int_init("slam_accuracy", par.slam_accuracy);
	subscribe_int_init("slam_ts_low", par.slam_ts_low);
	subscribe_int_init("slam_ts_high", par.slam_ts_high);
	subscribe_int_init("safty_level", par.safty_level);
	subscribe_int_init("order_action", par.order_action);
	subscribe_int_init("action_return", par.action_return);
	subscribe_int_init("speed_sv_x", par.speed_sv_x);
	subscribe_int_init("speed_sv_y", par.speed_sv_y);
	subscribe_float_init("speed_sv_z", par.speed_sv_z);
	subscribe_float_init("drv_pv_1", par.drv_pv_1);
	subscribe_float_init("drv_pv_2", par.drv_pv_2);
	subscribe_float_init("drv_pv_3", par.drv_pv_3);
	subscribe_float_init("drv_pv_4", par.drv_pv_4);

	subscribe_int_init("rc_sv_x", par.rc_sv_x);
	subscribe_int_init("rc_sv_y", par.rc_sv_y);
	subscribe_float_init("rc_sv_z", par.rc_sv_z);

	subscribe_bool_init("b_auto_mode", par.b_auto_mode);

	read_buffer_  = read_buf;
	write_buffer_ = write_buf;
}

void cls_lib::subscribe_int_init(const char *v_name, int &x)
{
	subscribe_int(v_name, x);
	um_int_.insert(std::pair<std::string, int*>(v_name, &x));
}
void cls_lib::subscribe_float_init(const char *v_name, float &x)
{
	subscribe_float(v_name, x);
	um_float_.insert(std::pair<std::string, float*>(v_name, &x));
}
void cls_lib::subscribe_bool_init(const char *v_name, bool &x)
{
	subscribe_bool(v_name, x);
	um_bool_.insert(std::pair<std::string, bool*>(v_name, &x));
}

const std::unordered_map<std::string, int *> cls_lib::GetIntMap()
{
	std::lock_guard<std::mutex> guard(g_mutex);
	return um_int_;
}

const std::unordered_map<std::string, float *> cls_lib::GetFloatMap()
{
	std::lock_guard<std::mutex> guard(g_mutex);
	return um_float_;
}

const std::unordered_map<std::string, bool *> cls_lib::GetBoolMap()
{
	std::lock_guard<std::mutex> guard(g_mutex);
	return um_bool_;
}

void *cls_lib::dl_adrress_get(void *_handle, const char *_name)
{
    char *error;
    void *ret = dlsym(_handle, _name);
    if ((error = dlerror()) != nullptr)
    {
        fprintf(stderr, "%s\n", error);
        _exit(-1);
    }
    return ret;
}
static size_t cnt = 0;
int cls_lib::tick(void)
{
	ReadMsgChange();
	WriteMsgChange();
	// printf("%ld --> tick\n", cnt++);
    return 0;
}

int cls_lib::RunTick(void)
{
	run_server_trig(tick);
	return 0;
}

void cls_lib::ReadMsgChange()
{
	if (read_buffer_ == nullptr)
		return;
	par.time = common_lib::LibTime::now();
	read_buffer_->push_back(par);
}

void cls_lib::WriteMsgChange()
{
	if (write_buffer_ == nullptr)
		return;
	if (write_buffer_->empty())
		return;
	while (write_buffer_->size() != 0)
	{
		par = write_buffer_->front();
		write_buffer_->pop_front();		
	}
}
